#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>

class ImageResizeNode : public rclcpp::Node
{
public:
    ImageResizeNode()
    : Node("image_resize_node")
    {
        // 订阅原始图像
        image_sub_ = this->create_subscription<sensor_msgs::msg::CompressedImage>(
            "/image", 10,
            std::bind(&ImageResizeNode::image_callback, this, std::placeholders::_1));

        // 发布压缩图像
        image_pub_ = this->create_publisher<sensor_msgs::msg::CompressedImage>("/image_resize", 10);
    }

private:
    void image_callback(const sensor_msgs::msg::CompressedImage::SharedPtr msg)
    {
        try
        {
            // 解码图像
            cv::Mat image = cv::imdecode(cv::Mat(msg->data), cv::IMREAD_COLOR);
            if (image.empty()) {
                RCLCPP_WARN(this->get_logger(), "❌ 解码失败，跳过该帧");
                return;
            }

            // 缩放图像为 160x120
            cv::Mat resized_image;
            cv::resize(image, resized_image, cv::Size(160, 120));

            // 编码为 JPEG
            std::vector<uchar> buffer;
            cv::imencode(".jpg", resized_image, buffer);

            // 构造 CompressedImage 消息
            auto compressed_msg = sensor_msgs::msg::CompressedImage();
            compressed_msg.header = msg->header;
            compressed_msg.format = "jpeg";
            compressed_msg.data = buffer;

            // 发布
            image_pub_->publish(compressed_msg);
            // RCLCPP_INFO(this->get_logger(), "✅ 发布压缩图像");
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "图像处理异常: %s", e.what());
        }
    }

    rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr image_sub_;
    rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr image_pub_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ImageResizeNode>());
    rclcpp::shutdown();
    return 0;
}
